/*
 * bsp_sbus.c
 *
 *  Created on: Apr 15, 2024
 *      Author: SDJX
 */



#include "bsp_sbus.h"
#include "bsp.h"
#include "string.h"
#include "bsp_motion.h"


#define SBUS_RECV_MAX    25
#define SBUS_START       0x0F
#define SBUS_END         0x00

// Parameters related to receiving data  接收数据相关参数
uint8_t sbus_start = 0;
uint8_t sbus_buf_index = 0;
uint8_t sbus_new_cmd = 0;

// data-caching mechanism  数据缓存
uint8_t inBuffer[SBUS_RECV_MAX] = {0};
uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE;

uint8_t sbus_data[SBUS_RECV_MAX] = {0};
int16_t g_sbus_channels[18] = {0};

//通道数据转换为控制参数
int16_t sbus_forward_back = 1024;
int16_t sbus_left_right = 1024;

//通道数据转控制数据
int16_t g_sbus_speed_X = 0;
int16_t g_sbus_speed_Y = 0;
int16_t g_sbus_speed_Z = 0;


/**
 * 将输入值映射到指定范围
 *
 * 本函数用于将一个输入值从原有的数值范围映射到新的数值范围。它保持了输入值相对比例不变，
 * 使得输入范围中的每个值都能对应输出范围中的一个唯一值。
 *
 * @param input 输入值，需要被映射的原始数值。
 * @param inMin 输入范围的最小值。
 * @param inMax 输入范围的最大值。
 * @param outMin 输出范围的最小值。
 * @param outMax 输出范围的最大值。
 * @return 映射后的输出值。
 */
int mapValue(int input, int inMin, int inMax, int outMin, int outMax)
{
    // 计算输入值在新范围中的映射值
    return (input - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
}


// Parses SBUS data into channel values  解析SBUS的数据，转化成通道数值。
static int SBUS_Parse_Data(void)
{
    g_sbus_channels[0]  = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF);
    g_sbus_channels[1]  = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF);
    g_sbus_channels[2]  = ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF);
    g_sbus_channels[3]  = ((sbus_data[5] >> 1 | sbus_data[6] << 7) & 0x07FF);
    g_sbus_channels[4]  = ((sbus_data[6] >> 4 | sbus_data[7] << 4) & 0x07FF);
    g_sbus_channels[5]  = ((sbus_data[7] >> 7 | sbus_data[8] << 1 | sbus_data[9] << 9) & 0x07FF);
    g_sbus_channels[6]  = ((sbus_data[9] >> 2 | sbus_data[10] << 6) & 0x07FF);
    g_sbus_channels[7]  = ((sbus_data[10] >> 5 | sbus_data[11] << 3) & 0x07FF);
    //#ifdef ALL_CHANNELS	//8通道开启此处
    g_sbus_channels[8]  = ((sbus_data[12] | sbus_data[13] << 8) & 0x07FF);
    g_sbus_channels[9]  = ((sbus_data[13] >> 3 | sbus_data[14] << 5) & 0x07FF);
	#ifdef ALL_CHANNELS		//10通道开启此处
    g_sbus_channels[10] = ((sbus_data[14] >> 6 | sbus_data[15] << 2 | sbus_data[16] << 10) & 0x07FF);
    g_sbus_channels[11] = ((sbus_data[16] >> 1 | sbus_data[17] << 7) & 0x07FF);
    g_sbus_channels[12] = ((sbus_data[17] >> 4 | sbus_data[18] << 4) & 0x07FF);
    g_sbus_channels[13] = ((sbus_data[18] >> 7 | sbus_data[19] << 1 | sbus_data[20] << 9) & 0x07FF);
    g_sbus_channels[14] = ((sbus_data[20] >> 2 | sbus_data[21] << 6) & 0x07FF);
    g_sbus_channels[15] = ((sbus_data[21] >> 5 | sbus_data[22] << 3) & 0x07FF);
    #endif

    // 安全检测，检测是否失联或者数据错误
    // Security detection to check for lost connections or data errors
    failsafe_status = SBUS_SIGNAL_OK;
    if (sbus_data[23] & (1 << 2))
    {
        failsafe_status = SBUS_SIGNAL_LOST;
        printf("SBUS_SIGNAL_LOST\n");
        // lost contact errors  遥控器失联错误
    }
    else if (sbus_data[23] & (1 << 3))
    {
        failsafe_status = SBUS_SIGNAL_FAILSAFE;
        printf("SBUS_SIGNAL_FAILSAFE\n");
        // data loss error  数据丢失错误
    }
    return failsafe_status;
}

// Receives SBUS cache data  接收SBUS的缓存数据
void SBUS_Reveive(uint8_t data)
{
    // If the protocol start flag is met, data is received  如果符合协议开始标志，则开始接收数据
    if (sbus_start == 0 && data == SBUS_START)
    {
        sbus_start = 1;
        sbus_new_cmd = 0;
        sbus_buf_index = 0;
        inBuffer[sbus_buf_index] = data;
        inBuffer[SBUS_RECV_MAX - 1] = 0xff;
    }
    else if (sbus_start)
    {
        sbus_buf_index++;
        inBuffer[sbus_buf_index] = data;
    }

    // Finish receiving a frame of data  完成接收一帧数据
    if (sbus_start & (sbus_buf_index >= (SBUS_RECV_MAX - 1)))
    {
        sbus_start = 0;
        if (inBuffer[SBUS_RECV_MAX - 1] == SBUS_END)
        {
            memcpy(sbus_data, inBuffer, SBUS_RECV_MAX);
            sbus_new_cmd = 1;
        }
    }
}

// SBUS receives and processes data handle  SBUS接收处理数据句柄
void SBUS_Handle(void)
{
//	static uint16_t stop_flag = 0;
//	static uint16_t stop_count = 100;
//	static uint8_t brake = 0;

	if (sbus_new_cmd)		//接收数据完成
    {
        int res = SBUS_Parse_Data();
        sbus_new_cmd = 0;
        if (res) return;
        //用于接收16通道遥控器配置--最大16通道
        #if SBUS_ALL_CHANNELS
        printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
               g_sbus_channels[0], g_sbus_channels[1], g_sbus_channels[2],
			   g_sbus_channels[3], g_sbus_channels[4], g_sbus_channels[5],
			   g_sbus_channels[6], g_sbus_channels[7], g_sbus_channels[8],
			   g_sbus_channels[9], g_sbus_channels[10], g_sbus_channels[11],
			   g_sbus_channels[12], g_sbus_channels[13], g_sbus_channels[14],
			   g_sbus_channels[15]);

        //用于接收10通道遥控器配置
		#elif SBUS_10_CHANNELS
        printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
                       g_sbus_channels[0], g_sbus_channels[1], g_sbus_channels[2],
        			   g_sbus_channels[3], g_sbus_channels[4], g_sbus_channels[5],
        			   g_sbus_channels[6], g_sbus_channels[7], g_sbus_channels[8],
        			   g_sbus_channels[9]);

        //用于接收8通道遥控器配置
        #else
        printf("%d,%d,%d,%d,%d,%d,%d,%d\r\n",
               g_sbus_channels[0], g_sbus_channels[1], g_sbus_channels[2],
			   g_sbus_channels[3], g_sbus_channels[4],g_sbus_channels[5],
			   g_sbus_channels[6], g_sbus_channels[7]);
        #endif

//        sbus_forward_back = mapValue(g_sbus_channels[2], 353, 1695, -3600, 3600); // 映射到-3000到+3000范围内
//        sbus_left_right = mapValue(g_sbus_channels[3], 353, 1695, -3600, 3600); // 映射到-3000到+3000范围内

		#if CAR1
        int16_t value_x = (g_sbus_channels[2] - SBUS_MIDDLE_VALUE);
		int16_t value_y = (g_sbus_channels[3] - SBUS_MIDDLE_VALUE);
		int16_t value_z = 0;

		if (value_x > -60 && value_x < 60) value_x = 0;
		if (value_y > -60 && value_y < 60) value_y = 0;
		if (value_z > -60 && value_z < 60) value_z = 0;

		if (value_x == 0 && value_y == 0 && value_z == 0)
		{
			if (stop_flag < stop_count)
			{
				stop_flag++;
				brake = g_sbus_channels[5] < 500 ? STOP_FREE:STOP_BRAKE;
				Motion_Stop(brake);

			}
			if (stop_flag == stop_count)
			{
				// 刹车超时切换到自由停止。
				stop_flag = stop_count + 1;
				Motion_Stop(STOP_FREE);
				//DEBUG("SBUS STOP TIMEOUT\n");
			}
		}
		else
		{
			g_sbus_speed_X = value_x * 1.2;
			g_sbus_speed_Y = value_y * 1.2;
			g_sbus_speed_Z = value_z * 5;

			Motion_Ctrl(g_sbus_speed_X, g_sbus_speed_Y, g_sbus_speed_Z);

			stop_flag = 0;
		}

		//控制弹射电机继电器，当传感器检测到一定位置时，停止不动，通道0右侧时不受传感器控制；
		if(g_sbus_channels[0] <= 800)
		{
			if(0 == HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin))
			{
				RELAYS2_OFF();
			}
			else
			{
				RELAYS2_ON();
			}
		}
		else if(g_sbus_channels[0] >= 1300)
		{

			RELAYS2_ON();
		}
		else
		{

			RELAYS2_OFF();
		}

		//控制电磁阀动作
		if(g_sbus_channels[7] <= 600)
		{
			RELAYS1_ON();
		}
		else
		{
			RELAYS1_OFF();
		}
		//控制炮车无刷的开关
		if(g_sbus_channels[4] <= 600)
		{
			RELAYS3_ON();
			RELAYS4_ON();
		}
		else
		{
			RELAYS3_OFF();
			RELAYS4_OFF();
		}

		#elif CAR2
		#elif CAR3
		#elif CAR4
		#else
//        // 控制小车运动
//		int16_t value_x = (g_sbus_channels[2] - SBUS_MIDDLE_VALUE);
//		int16_t value_y = -(g_sbus_channels[3] - SBUS_MIDDLE_VALUE);
//		int16_t value_z = (g_sbus_channels[0] - SBUS_MIDDLE_VALUE);
//
//		if (value_x > -60 && value_x < 60) value_x = 0;
//		if (value_y > -60 && value_y < 60) value_y = 0;
//		if (value_z > -60 && value_z < 60) value_z = 0;
//
//		if (value_x == 0 && value_y == 0 && value_z == 0)
//		{
//			if (stop_flag < stop_count)
//			{
//				stop_flag++;
//				brake = g_sbus_channels[5] < 500 ? STOP_FREE:STOP_BRAKE;
//				Motion_Stop(brake);
//
//			}
//			if (stop_flag == stop_count)
//			{
//				// 刹车超时切换到自由停止。
//				stop_flag = stop_count + 1;
//				Motion_Stop(STOP_FREE);
//				//DEBUG("SBUS STOP TIMEOUT\n");
//			}
//		}
//		else
//		{
////			switch (Motion_Get_Car_Type())
//			switch (CAR_MECANUM)
//			{
//			case CAR_MECANUM:
//			{
//				g_sbus_speed_X = value_x * 1.2;
//				g_sbus_speed_Y = value_y * 1.2;
//				g_sbus_speed_Z = value_z * 5;
//
//				Motion_Ctrl(g_sbus_speed_X, g_sbus_speed_Y, g_sbus_speed_Z);
//				break;
//			}
//			case CAR_MECANUM_MAX:
//			{
//				g_sbus_speed_X = value_x * 1.2;
//				g_sbus_speed_Y = value_y * 1.2;
//				g_sbus_speed_Z = value_z * 5;
//				Motion_Ctrl(g_sbus_speed_X, g_sbus_speed_Y, g_sbus_speed_Z);
//				break;
//			}
//			default:
//				break;
//
//			}
//
//
//			stop_flag = 0;
//		}
		#endif
    }
}



//void SBUS_Motion(void)
//{
//	int inputValue = 1000; // 输入值范围为0-2000
//	int outputValue = mapValue(inputValue, 353, 1695, -3000, 3000); // 映射到-3000到+3000范围内
//
//	printf("Mapped value: %d\n", outputValue);
//}

